#include "Game Engine\Header\Vehicle.h"
#include "Game Engine\Header\TextureMgr.h"
#include "Source\Header\Effects.h"

#ifndef M_PI
#define M_PI       3.14159265358979323846
#endif

#ifndef M_PI_2
#define M_PI_2     1.57079632679489661923
#endif

#ifndef M_PI_4
#define M_PI_4     0.785398163397448309616
#endif

#define KEYPRESSED( key )	( ( ( GetAsyncKeyState( key ) & 0x8000 ) ) ? 1 : 0 )

#define CUBE_HALF_EXTENTS 1

Vehicle::Vehicle() :
mCarChassis(0),
mLiftBody(0),
mForkBody(0),
mDefaultContactProcessingThreshold( BT_LARGE_FLOAT )
{
	mPosition = D3DXVECTOR3( 0.0f, 0.0f, 0.0f );

	mVehicle = 0;
	mWheelShape = 0;
	mWheelDirectionCS0 = btVector3( 0, -1, 0 );
	mWheelAxleCS = btVector3( -1, 0, 0 );

	mSuspensionRestLength = btScalar( 0.6 );

	mRightIndex = 0;
	mUpIndex = 1;
	mForwardIndex = 2;

	mEngineForce = 0.f;

	mDefaultBreakingForce = 10.f;
	mBreakingForce = 100.f;

	mMaxEngineForce = 1000.0f;//this should be engine/velocity dependent
	mMaxBreakingForce = 100.f;

	mVehicleSteering = 0.f;
	mSteeringIncrement = 0.0008f;
	mSteeringClamp = 0.3f;
	mWheelRadius = 0.5f;
	mWheelWidth = 0.4f;
	mWheelFriction = 10;//BT_LARGE_FLOAT;
	mSuspensionStiffness = 20.f;
	mSuspensionDamping = 2.3f;
	mSuspensionCompression = 4.4f;
	mRollInfluence = 0.1f;//1.0f;

	mNumChassis = 1;
	mNumLifts	= 0;
	mNumForks	= 3;

	mLiftWidth		= 0.0f;
	mLiftLength		= 0.0f;
	mLiftHeight		= 0.0f;

	mPitch		= 0.0f;
	mYaw		= 0.0f;
	mRoll		= 0.0f;

	mIsForkLift = false;
}

Vehicle::~Vehicle()
{
}

void Vehicle::Init( ID3D10Device* device, btDynamicsWorld* world, std::wstring chassis,
	std::wstring wheels, ID3D10Effect* fx, ID3D10ShaderResourceView* cubeMap, float mass,
	bool useShadows )
{
	md3dDevice = device;

	mDynamicsWorld = world;

	mCubeMap = cubeMap;
	mFX = fx;

	mUseShadows = useShadows;

	mMass = mass;

	if( wheels != L"" )
	{
		mWheelMesh = new Mesh();
		mWheelMesh->Init( md3dDevice, wheels, fx, mUseShadows );
		mWheelMesh->SetCubeMap( cubeMap );

		//mCarChassis->setDamping(0.2,0.2);

		// Create the wheel shapes based on the size of the wheel mesh.
		D3DXVECTOR3 wheelMin;
		D3DXVECTOR3 wheelMax;

		mWheelMesh->GetAABB( wheelMin, wheelMax );

		mWheelWidth = abs( wheelMax.x - wheelMin.x );
		mWheelRadius = abs( wheelMax.y - wheelMin.y ) / 2;

		mWheelShape = new btCylinderShapeX( btVector3( mWheelWidth, mWheelRadius, mWheelRadius ) );
	}

	ClientResetScene();

	// Create vehicle
	mVehicleRayCaster = new btDefaultVehicleRaycaster( mDynamicsWorld );
	mCarChassis ? mVehicle = new btRaycastVehicle( mTuning, mCarChassis, mVehicleRayCaster ) :
		mVehicle = new btRaycastVehicle( mTuning, mPlaneChassis, mVehicleRayCaster );

	// Never deactivate the vehicle
	mCarChassis ? mCarChassis->setActivationState( DISABLE_DEACTIVATION ) :
		mPlaneChassis->setActivationState( DISABLE_DEACTIVATION );

	mDynamicsWorld->addVehicle( mVehicle );

	// Choose coordinate system
	mVehicle->setCoordinateSystem( mRightIndex, mUpIndex, mForwardIndex );

	if( wheels != L"" )
	{
		float connectionHeight = mChassisHeight + 0.6f;

		bool isFrontWheel = true;

		// Add the wheel to the vehicle based on the connection point and if it's
		// a front wheel or not.
		btVector3 connectionPointCS0( mChassisWidth - ( 0.3 * mWheelWidth ),
			connectionHeight, mChassisLength - mWheelRadius );
		mVehicle->addWheel( connectionPointCS0, mWheelDirectionCS0, mWheelAxleCS,
			mSuspensionRestLength, mWheelRadius, mTuning, isFrontWheel );

		connectionPointCS0 = btVector3( -mChassisWidth + ( 0.3 * mWheelWidth ),
			connectionHeight, mChassisLength - mWheelRadius );
		mVehicle->addWheel( connectionPointCS0, mWheelDirectionCS0, mWheelAxleCS,
			mSuspensionRestLength, mWheelRadius, mTuning, isFrontWheel );

		isFrontWheel = false;
		connectionPointCS0 = btVector3( -mChassisWidth + ( 0.3 * mWheelWidth ),
			connectionHeight, -mChassisLength + mWheelRadius );
		mVehicle->addWheel( connectionPointCS0, mWheelDirectionCS0, mWheelAxleCS,
			mSuspensionRestLength, mWheelRadius, mTuning, isFrontWheel );

		connectionPointCS0 = btVector3( mChassisWidth - (0.3 * mWheelWidth ),
			connectionHeight, -mChassisLength + mWheelRadius );
		mVehicle->addWheel( connectionPointCS0, mWheelDirectionCS0, mWheelAxleCS,
			mSuspensionRestLength, mWheelRadius, mTuning, isFrontWheel );

		// Set the wheel settings.
		for( int i = 0; i < mVehicle->getNumWheels(); i++ )
		{
			btWheelInfo& wheel = mVehicle->getWheelInfo(i);
			wheel.m_suspensionStiffness = mSuspensionStiffness;
			wheel.m_wheelsDampingRelaxation = mSuspensionDamping;
			wheel.m_wheelsDampingRelaxation = mSuspensionCompression;
			wheel.m_frictionSlip = mWheelFriction;
			wheel.m_rollInfluence = mRollInfluence;
		}
	}
}

void Vehicle::SetForkLift( std::wstring lift, std::wstring forkSides,
	std::wstring forkBack, std::wstring connector )
{
	mIsForkLift = true;

	mConnectMesh = new Mesh;
	mConnectMesh->Init( md3dDevice, connector, mFX, mUseShadows );
	mConnectMesh->SetCubeMap( mCubeMap );

	D3DXVECTOR3 min;
	D3DXVECTOR3 max;

	mConnectMesh->GetAABB( min, max );
	mConnectWidth = abs( max.x - min.x ) / 2.0f;
	mConnectHeight = abs( max.y - min.y ) / 2.0f;
	mConnectLength = abs( max.z - min.z ) / 2.0f;

	mLiftMesh = new Mesh;
	mLiftMesh->Init( md3dDevice, lift, mFX, mUseShadows );
	mLiftMesh->SetCubeMap( mCubeMap );

	mForkMesh.push_back( new Mesh );
	mForkMesh[0]->Init( md3dDevice, forkBack, mFX, mUseShadows );
	mForkMesh[0]->SetCubeMap( mCubeMap );

	mForkMesh.push_back( new Mesh );
	mForkMesh[1]->Init( md3dDevice, forkSides, mFX, mUseShadows );
	mForkMesh[1]->SetCubeMap( mCubeMap );

	mForkMesh.push_back( new Mesh );
	mForkMesh[2]->Init( md3dDevice, forkSides, mFX, mUseShadows );
	mForkMesh[2]->SetCubeMap( mCubeMap );

	btTransform connectTrans;
	connectTrans.setIdentity();

	// Sets the position based on size of shape plus size of chassis
	btCollisionShape* connectShape = new btBoxShape( btVector3( mConnectWidth, mConnectHeight,
		mConnectLength ) );

	connectTrans.setOrigin( btVector3( 0.0f, mChassisHeight * 2.0f,
				mChassisLength + mConnectLength ) );

	mConnectBody = CreateRigidBody( mDynamicsWorld, 1.0f, connectTrans, connectShape );//chassisShape);

	// Create the lift shape based on the size of the lift mesh.
	D3DXVECTOR3 liftMin;
	D3DXVECTOR3 liftMax;

	mLiftMesh->GetAABB( liftMin, liftMax );

	mLiftWidth = abs( liftMax.x - liftMin.x ) / 2;
	mLiftHeight = abs( liftMax.y - liftMin.y ) / 2;
	mLiftLength = abs( liftMax.z - liftMin.z ) / 2;

	btCollisionShape* liftShape = new btBoxShape( btVector3( mLiftWidth, mLiftHeight,
		mLiftLength ) );
	btTransform liftTrans;

	// Sets the position based on size of shape plus size of chassis
	mLiftStartPos = btVector3( mPosition.x, mPosition.y + mLiftHeight + mChassisHeight,
		mPosition.z + mLiftLength + mChassisLength + ( mConnectLength * 2 ) );

	liftTrans.setIdentity();
	liftTrans.setOrigin( mLiftStartPos );
	mLiftBody = CreateRigidBody( mDynamicsWorld, 10, liftTrans, liftShape );

	// Creates a hinge for the lift so it can rotate back and forth.
	btTransform localA, localB;
	localA.setIdentity();
	localB.setIdentity();
	localA.getBasis().setEulerZYX( 0, M_PI_2, 0 );
	localA.setOrigin( btVector3( 0.0, mChassisHeight * 2,
		mPosition.z + mChassisLength + ( mConnectLength * 2 ) ) );
	localB.getBasis().setEulerZYX( 0, M_PI_2, 0);
	localB.setOrigin( btVector3( 0.0, ( mChassisHeight * 2 ) -
		( mPosition.y + mLiftHeight + mChassisHeight ), -mLiftLength ) );
	mLiftHinge = new btHingeConstraint( *mCarChassis, *mLiftBody, localA, localB );
	//mLiftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
	mLiftHinge->setLimit( 0.0f, 0.0f );

	// Make sure to add the hinge to the dynamics world.
	mDynamicsWorld->addConstraint( mLiftHinge, true );

	// Create a compound shape for the fork.
	btCompoundShape* forkCompound = new btCompoundShape();
	btTransform forkLocalTrans;

	// Create the fork based on the size of each fork mesh
	// and add them to the compound shape
	for( UINT i = 0; i < mNumForks; i ++ )
	{
		D3DXVECTOR3 forkMin;
		D3DXVECTOR3 forkMax;

		mForkMesh[i]->GetAABB( forkMin, forkMax );

		mForkWidth.push_back( abs( forkMax.x - forkMin.x ) / 2 );
		mForkHeight.push_back( abs( forkMax.y - forkMin.y ) / 2 );
		mForkLength.push_back( abs( forkMax.z - forkMin.z ) / 2 );

		btCollisionShape* forkShape = new btBoxShape( btVector3( mForkWidth[i],
			mForkHeight[i], mForkLength[i] ) );
		mCollisionShapes.push_back( forkShape );
		mCollisionShapes.push_back( forkCompound );
		forkLocalTrans.setIdentity();
		if( i == 1 )
			forkLocalTrans.setOrigin( btVector3( -( mForkWidth[0] - mForkWidth[1] ),
			-( mForkHeight[0] - mForkHeight[1] ), ( mForkLength[0] + mForkLength[1] ) ) );
		else if( i == 2 )
			forkLocalTrans.setOrigin( btVector3( ( mForkWidth[0] - mForkWidth[1] ),
			-( mForkHeight[0] - mForkHeight[1] ), ( mForkLength[0] + mForkLength[1] ) ) );
		forkCompound->addChildShape( forkLocalTrans, forkShape );
	}

	btTransform forkTrans;

	// Sets the position based on size of shape plus size of chassis
	mForkStartPos = btVector3 ( mPosition.x, mPosition.y + mChassisHeight + mForkHeight[0],
		mPosition.z + mChassisLength + mConnectLength + ( mForkLength[0] + mForkLength[1] ) );

	forkTrans.setIdentity();
	forkTrans.setOrigin( mForkStartPos );
	mForkBody = CreateRigidBody( mDynamicsWorld, 5, forkTrans, forkCompound );

	// Creates a slider constraint so the fork can move up and down on the lift.
	localA.setIdentity();
	localB.setIdentity();
	localA.getBasis().setEulerZYX( 0, 0, M_PI_2 );
	localA.setOrigin( btVector3( 0.0f, -( mLiftHeight - mForkHeight[0] ), mLiftLength ) );
	localB.getBasis().setEulerZYX( 0, 0, M_PI_2 );
	localB.setOrigin( btVector3( 0.0, 0.0, -mForkLength[0] ) );
	mForkSlider = new btSliderConstraint( *mLiftBody, *mForkBody, localA, localB, true );
	mForkSlider->setLowerLinLimit( 0.1f );
	mForkSlider->setUpperLinLimit( 0.1f );
	//mForkSlider->setLowerAngLimit(-LIFT_EPS);
	//mForkSlider->setUpperAngLimit(LIFT_EPS);
	mForkSlider->setLowerAngLimit( 0.0f );
	mForkSlider->setUpperAngLimit( 0.0f );

	// Make sure to add the slider to the dynamics world.
	mDynamicsWorld->addConstraint( mForkSlider, true );
}

D3DXVECTOR3& Vehicle::GetPosition()
{
	return mPosition;
}

void Vehicle::SetPosition( D3DXVECTOR3 pos )
{
	mPosition = pos;

	btTransform chassisTrans;
	chassisTrans.setIdentity();
	chassisTrans.setOrigin( btVector3( BTVector3( mPosition ) ) );
	mCarChassis ? mCarChassis->setCenterOfMassTransform( chassisTrans ) :
		mPlaneChassis->setCenterOfMassTransform( chassisTrans );
	if( mVehicle )
	{
		mVehicle->resetSuspension();
		for( int i = 0; i < mVehicle->getNumWheels(); i++ )
		{
			//synchronize the wheels with the (interpolated) chassis worldtransform
			mVehicle->updateWheelTransform( i, true );
		}
	}

	if( mIsForkLift )
	{
		btVector3 liftPos = btVector3( mPosition.x, mPosition.y + mLiftHeight + mChassisHeight,
			mPosition.z + mLiftLength + mChassisLength + ( mConnectLength * 2 ) );

		btVector3 forkPos = btVector3 ( mPosition.x, mPosition.y + mChassisHeight + mForkHeight[0],
			mPosition.z + mChassisLength + mConnectLength + ( mForkLength[0] + mForkLength[1] ) );

		btTransform liftTrans;
		liftTrans.setIdentity();
		liftTrans.setOrigin( liftPos );
		mLiftBody->setCenterOfMassTransform( liftTrans );

		btTransform forkTrans;
		forkTrans.setIdentity();
		forkTrans.setOrigin( forkPos );
		mForkBody->setCenterOfMassTransform( forkTrans );
	}
}

D3DXVECTOR3 Vehicle::GetRotation()
{
	return D3DXVECTOR3( mPitch, mYaw, mRoll );
}

void Vehicle::SetRotation( Axis axis, float angle, bool clockwise )
{
	//Object::Rotate( d, clockwise );

	clockwise ? angle = -angle * ( PI / 180.0f ) : angle = angle * ( PI / 180.0f );
	if( angle >= 2.0f * PI )
		angle = 0.0f;

	btTransform chassisTrans;
	chassisTrans.setIdentity();
	chassisTrans.setOrigin( btVector3( BTVector3( mPosition ) ) );
	switch( axis )
	{
	case X:
		mPitch = angle;
		break;
	case Y:
		mYaw = angle;
		break;
	case Z:
		mRoll = angle;
		break;
	}

	chassisTrans.setRotation( btQuaternion( mYaw, mPitch, mRoll ) );
	mCarChassis ? mCarChassis->setWorldTransform( chassisTrans ) : mPlaneChassis->setWorldTransform( chassisTrans );
}

void Vehicle::SetGravity( D3DXVECTOR3 gravity )
{
	mCarChassis ? mCarChassis->setGravity( BTVector3( gravity ) ) :
		mPlaneChassis->setGravity( BTVector3( gravity ) );

	if( mIsForkLift )
	{
		mLiftBody->setGravity( BTVector3( gravity ) );
		mForkBody->setGravity( BTVector3( gravity ) );
		mConnectBody->setGravity( BTVector3( gravity ) );
	}
}

void Vehicle::Draw( D3DXMATRIX& lightViewProj )
{
	btMotionState* ms = mVehicle->getRigidBody()->getMotionState();
	if(ms != NULL)
		mWorld = BT2DX_MATRIX( *ms );
}

void Vehicle::DrawToShadowMap( ID3D10EffectMatrixVariable* mfxBuildShadowMapLightWVPVar,
	ID3D10EffectShaderResourceVariable* mfxBuildShadowMapDiffuseMapVar,
	ID3D10EffectPass* pass,	D3DXMATRIX& lightViewProj )
{
	D3DXMATRIX WVP;
	WVP = mWorld * lightViewProj;
	mfxBuildShadowMapLightWVPVar->SetMatrix((float*)&WVP);
	mChassisMesh->DrawToShadowMap( mfxBuildShadowMapDiffuseMapVar, pass );
}

// Reset the vehicle and load potitions.
void Vehicle::ClientResetScene()
{
	//mVehicleSteering = 0.f;
	//mBreakingForce = mDefaultBreakingForce;
	//mEngineForce = 0.f;

	//btTransform chassisTrans;
	//chassisTrans.setIdentity();
	//chassisTrans.setOrigin( btVector3( BTVector3( mPosition ) ) );
	//mCarChassis->setCenterOfMassTransform( chassisTrans );
	//mCarChassis->setLinearVelocity( btVector3( 0, 0, 0 ) );
	//mCarChassis->setAngularVelocity( btVector3( 0 , 0, 0 ) );
	//mDynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(mCarChassis->
	//	getBroadphaseHandle(), mDynamicsWorld->getDispatcher());
	//if( mVehicle )
	//{
	//	mVehicle->resetSuspension();
	//	for( int i = 0; i < mVehicle->getNumWheels(); i++ )
	//	{
	//		//synchronize the wheels with the (interpolated) chassis worldtransform
	//		mVehicle->updateWheelTransform( i, true );
	//	}
	//}

	//if( mIsForkLift )
	//{
	//	btTransform liftTrans;
	//	liftTrans.setIdentity();
	//	liftTrans.setOrigin( mLiftStartPos );
	//	mLiftBody->activate();
	//	mLiftBody->setCenterOfMassTransform( liftTrans );
	//	mLiftBody->setLinearVelocity( btVector3( 0, 0, 0 ) );
	//	mLiftBody->setAngularVelocity( btVector3( 0, 0, 0 ) );

	//	btTransform forkTrans;
	//	forkTrans.setIdentity();
	//	forkTrans.setOrigin( mForkStartPos );
	//	mForkBody->activate();
	//	mForkBody->setCenterOfMassTransform( forkTrans );
	//	mForkBody->setLinearVelocity( btVector3( 0, 0, 0 ) );
	//	mForkBody->setAngularVelocity( btVector3( 0, 0, 0 ) );

	//	//mLiftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
	//	mLiftHinge->setLimit( 0.0f, 0.0f );
	//	mLiftHinge->enableAngularMotor( false, 0, 0 );

	//	mForkSlider->setLowerLinLimit( 0.1f );
	//	mForkSlider->setUpperLinLimit( 0.1f );
	//	mForkSlider->setPoweredLinMotor( false );
	//}
}

// Locks the lift hinge based on its current position.
void Vehicle::LockLiftHinge( void )
{
	btScalar hingeAngle = mLiftHinge->getHingeAngle();
	btScalar lowLim = mLiftHinge->getLowerLimit();
	btScalar hiLim = mLiftHinge->getUpperLimit();
	mLiftHinge->enableAngularMotor( false, 0, 0 );
	if( hingeAngle < lowLim )
	{
		//mLiftHinge->setLimit( lowLim, lowLim + LIFT_EPS );
		mLiftHinge->setLimit( lowLim, lowLim );
	}
	else if( hingeAngle > hiLim )
	{
		//mLiftHinge->setLimit( hiLim - LIFT_EPS, hiLim );
		mLiftHinge->setLimit( hiLim, hiLim );
	}
	else
	{
		//mLiftHinge->setLimit( hingeAngle - LIFT_EPS, hingeAngle + LIFT_EPS );
		mLiftHinge->setLimit( hingeAngle, hingeAngle );
	}
	return;
} // Vehicle::lockLiftHinge()

// Locks the fork slider based on its current position.
void Vehicle::LockForkSlider( void )
{
	btScalar linDepth = mForkSlider->getLinearPos();
	btScalar lowLim = mForkSlider->getLowerLinLimit();
	btScalar hiLim = mForkSlider->getUpperLinLimit();
	mForkSlider->setPoweredLinMotor( false );
	if( linDepth <= lowLim )
	{
		mForkSlider->setLowerLinLimit( lowLim );
		mForkSlider->setUpperLinLimit( lowLim );
	}
	else if( linDepth > hiLim )
	{
		mForkSlider->setLowerLinLimit( hiLim );
		mForkSlider->setUpperLinLimit( hiLim );
	}
	else
	{
		mForkSlider->setLowerLinLimit( linDepth );
		mForkSlider->setUpperLinLimit( linDepth );
	}
	return;
} // Vehicle::lockForkSlider()

D3DXMATRIX& Vehicle::GetWorld()
{
	return mWorld;
}

btRaycastVehicle* Vehicle::GetVehicle()
{
	return mVehicle;
}